{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ImuFactor\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ImuFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "### ImuFactor (5-way Factor)\n",
    "\n",
    "The `ImuFactor` is the standard GTSAM factor for incorporating preintegrated IMU measurements into a factor graph. It's a 5-way factor connecting:\n",
    "\n",
    "1.  Pose at time $i$ (`Pose3`)\n",
    "2.  Velocity at time $i$ (`Vector3`)\n",
    "3.  Pose at time $j$ (`Pose3`)\n",
    "4.  Velocity at time $j$ (`Vector3`)\n",
    "5.  IMU Bias at time $i$ (`imuBias::ConstantBias`)\n",
    "\n",
    "It takes a `PreintegratedImuMeasurements` object, which summarizes the IMU readings between times $i$ and $j$. The factor's error function measures the discrepancy between the relative motion predicted by the preintegrated measurements (corrected for the *current* estimate of the bias at time $i$) and the relative motion implied by the state variables ($Pose_i, Vel_i, Pose_j, Vel_j$) connected to the factor.\n",
    "\n",
    "### ImuFactor2: NavState Variant\n",
    "\n",
    "The `ImuFactor2` is ternary variant of the `ImuFactor` that operates directly on `NavState` objects instead of separate `Pose3` and `Vector3` variables for pose and velocity. This simplifies the factor graph by reducing the number of connected variables and can make the graph more efficient to optimize.\n",
    "\n",
    "Instead of connecting five variables (`Pose_i`, `Vel_i`, `Pose_j`, `Vel_j`, `Bias_i`), the `ImuFactor2` connects three:\n",
    "\n",
    "1. `NavState` at time $i$ (`NavState` combines pose and velocity)\n",
    "2. `NavState` at time $j`\n",
    "3. IMU Bias at time $i$ (`imuBias::ConstantBias`)\n",
    "\n",
    "### Modeling Bias\n",
    "\n",
    "Both factors assume that the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. That is typically a very good assumption, as bias evolves slowly over time.\n",
    "\n",
    "The factors do *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
      "\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
      "\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Formulation\n",
    "\n",
    "Let $X_i = (R_i, p_i, v_i)$ be the state (Attitude, Position, Velocity) at time $i$, and $b_i$ be the bias estimate at time $i$.\n",
    "Let $X_j = (R_j, p_j, v_j)$ be the state at time $j$.\n",
    "\n",
    "The `PreintegratedImuMeasurements` object (`pim`) provides:\n",
    "- $\\Delta \\tilde{R}_{ij}(b_{est})$, $\\Delta \\tilde{p}_{ij}(b_{est})$, $\\Delta \\tilde{v}_{ij}(b_{est})$: Preintegrated measurements calculated using the fixed bias estimate $b_{est}$ (`pim.biasHat()`).\n",
    "- Jacobians of the $\\Delta$ terms with respect to the bias.\n",
    "\n",
    "The factor first uses the `pim` object's `predict` method (or equivalent calculations) to find the predicted state $X_{j,pred}$ based on $X_i$ and the *current* estimate of the bias $b_i$ from the factor graph values:\n",
    "$$ X_{j,pred} = \\text{pim.predict}(X_i, b_i) $$ \n",
    "This prediction internally applies first-order corrections to the stored $\\Delta$ values based on the difference between $b_i$ and $b_{est}$.\n",
    "\n",
    "The factor's 9-dimensional error vector $e$ is then calculated as the difference between the predicted state $X_{j,pred}$ and the actual state $X_j$ in the tangent space of $X_{j,pred}$:\n",
    "$$ e = \\text{Logmap}_{X_{j,pred}}(X_j) = X_{j,pred}^{-1} \\otimes X_j \\quad \\text{(Conceptual Lie notation)} $$ \n",
    "Or, more explicitly using the `NavState::Logmap` definition:\n",
    "$$ e = \\text{NavState::Logmap}(X_{j,pred}.inverse() * X_j) $$ \n",
    "This error vector has components corresponding to errors in rotation (3), position (3), and velocity (3)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructor**: `ImuFactor(keyPose_i, keyVel_i, keyPose_j, keyVel_j, keyBias_i, pim)`: Creates the factor, linking the five state/bias keys and providing the preintegrated measurements.\n",
    "- **`preintegratedMeasurements()`**: Returns a const reference to the stored `PreintegratedImuMeasurements` object.\n",
    "- **`evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i)`**: Calculates the 9-dimensional error vector given the current values of the connected variables. Also computes Jacobians if requested.\n",
    "- **`noiseModel()`**: Returns the noise model associated with the preintegrated measurement uncertainty (derived from `pim.preintMeasCov()`).\n",
    "- **`print` / `equals`**: Standard factor methods."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Create parameters, a PIM object, define keys, and construct the factor."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Created ImuFactor:\n",
      "ImuFactor(x0,v0,x1,v1,b0)\n",
      "preintegrated measurements:\n",
      "\n",
      "    deltaTij = 0.1\n",
      "    deltaRij.ypr = ( 0 -0  0)\n",
      "    deltaPij =        0        0 -0.04905\n",
      "    deltaVij =      0      0 -0.981\n",
      "    gyrobias = 0 0 0\n",
      "    acc_bias = 0 0 0\n",
      "\n",
      "    preintMeasCov \n",
      "[       1e-05            0            0            0  1.39793e-07            0            0   4.4145e-06            0\n",
      "           0        1e-05            0 -1.39793e-07            0            0  -4.4145e-06            0            0\n",
      "           0            0        1e-05            0            0            0            0            0            0\n",
      "           0 -1.39793e-07            0  3.32969e-06            0            0  5.00974e-05            0            0\n",
      " 1.39793e-07            0            0            0  3.32969e-06            0            0  5.00974e-05            0\n",
      "           0            0            0            0            0    3.326e-06            0            0        5e-05\n",
      "           0  -4.4145e-06            0  5.00974e-05            0            0   0.00100274            0            0\n",
      "  4.4145e-06            0            0            0  5.00974e-05            0            0   0.00100274            0\n",
      "           0            0            0            0            0        5e-05            0            0        0.001]\n",
      "  noise model sigmas: 0.00316228 0.00316228 0.00316228 0.00182474 0.00182474 0.00182373  0.0316661  0.0316661  0.0316228\n",
      "\n",
      "Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
      "Factor error (0.5 * ||error||^2_Sigma): 0.0\n"
     ]
    }
   ],
   "source": [
    "from gtsam import PreintegrationParams, PreintegratedImuMeasurements, ImuFactor\n",
    "from gtsam import NonlinearFactorGraph, Values, NavState, Pose3\n",
    "from gtsam.symbol_shorthand import X,V,B\n",
    "from gtsam.imuBias import ConstantBias\n",
    "import numpy as np\n",
    "\n",
    "# 1. Create Parameters and PIM (as in PreintegratedImuMeasurements example)\n",
    "params = PreintegrationParams.MakeSharedU(9.81)\n",
    "accel_noise_sigma = 0.1\n",
    "gyro_noise_sigma = 0.01\n",
    "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
    "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
    "bias_hat = ConstantBias() # Assume zero bias used for preintegration\n",
    "pim = PreintegratedImuMeasurements(params, bias_hat)\n",
    "\n",
    "# Integrate some dummy measurements\n",
    "dt = 0.01\n",
    "acc_meas = np.array([0.0, 0.0, -9.81]) # Stationary\n",
    "gyro_meas = np.array([0.0, 0.0, 0.0]) # Stationary\n",
    "for _ in range(10):\n",
    "    pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
    "\n",
    "# 2. Symbolic Keys will be X(0), V(0), X(1), V(1), B(0)\n",
    "\n",
    "# 3. Create the ImuFactor\n",
    "# The noise model is automatically derived from pim.preintMeasCov()\n",
    "imu_factor = ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)\n",
    "\n",
    "print(\"Created ImuFactor:\")\n",
    "imu_factor.print()\n",
    "\n",
    "# 4. Example: Evaluate error with perfect states (should be near zero)\n",
    "graph = NonlinearFactorGraph()\n",
    "graph.add(imu_factor)\n",
    "\n",
    "values = Values()\n",
    "pose_i = Pose3() # Identity\n",
    "vel_i = np.zeros(3)\n",
    "bias_i = ConstantBias() # Zero bias\n",
    "\n",
    "# Predict state j using the PIM and the *same* bias used for integration\n",
    "nav_state_i = NavState(pose_i, vel_i)\n",
    "nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat\n",
    "pose_j = nav_state_j.pose()\n",
    "vel_j = nav_state_j.velocity()\n",
    "\n",
    "values.insert(X(0), pose_i)\n",
    "values.insert(V(0), vel_i)\n",
    "values.insert(X(1), pose_j)\n",
    "values.insert(V(1), vel_j)\n",
    "values.insert(B(0), bias_i)\n",
    "\n",
    "error_vector = imu_factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i)\n",
    "print(\"\\nError vector (should be near zero):\", error_vector)\n",
    "print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also use `ImuFactor2`, with `NavState`, giving exactly the same result:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Created ImuFactor2:\n",
      "ImuFactor2(x0,x1,b0)\n",
      "preintegrated measurements:\n",
      "\n",
      "    deltaTij = 0.1\n",
      "    deltaRij.ypr = ( 0 -0  0)\n",
      "    deltaPij =        0        0 -0.04905\n",
      "    deltaVij =      0      0 -0.981\n",
      "    gyrobias = 0 0 0\n",
      "    acc_bias = 0 0 0\n",
      "\n",
      "    preintMeasCov \n",
      "[       1e-05            0            0            0  1.39793e-07            0            0   4.4145e-06            0\n",
      "           0        1e-05            0 -1.39793e-07            0            0  -4.4145e-06            0            0\n",
      "           0            0        1e-05            0            0            0            0            0            0\n",
      "           0 -1.39793e-07            0  3.32969e-06            0            0  5.00974e-05            0            0\n",
      " 1.39793e-07            0            0            0  3.32969e-06            0            0  5.00974e-05            0\n",
      "           0            0            0            0            0    3.326e-06            0            0        5e-05\n",
      "           0  -4.4145e-06            0  5.00974e-05            0            0   0.00100274            0            0\n",
      "  4.4145e-06            0            0            0  5.00974e-05            0            0   0.00100274            0\n",
      "           0            0            0            0            0        5e-05            0            0        0.001]\n",
      "  noise model sigmas: 0.00316228 0.00316228 0.00316228 0.00182474 0.00182474 0.00182373  0.0316661  0.0316661  0.0316228\n",
      "\n",
      "Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
      "Factor error (0.5 * ||error||^2_Sigma): 0.0\n"
     ]
    }
   ],
   "source": [
    "from gtsam import ImuFactor2\n",
    "\n",
    "# 1. Create the ImuFactor2\n",
    "# The noise model is automatically derived from pim.preintMeasCov()\n",
    "imu_factor2 = ImuFactor2(X(0), X(1), B(0), pim)\n",
    "\n",
    "print(\"Created ImuFactor2:\")\n",
    "imu_factor2.print()\n",
    "\n",
    "# 2. Example: Evaluate error with perfect states (should be near zero)\n",
    "graph = NonlinearFactorGraph()\n",
    "graph.add(imu_factor2)\n",
    "\n",
    "values = Values()\n",
    "nav_state_i = NavState(pose_i, vel_i)\n",
    "nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat\n",
    "\n",
    "values.insert(X(0), nav_state_i)\n",
    "values.insert(X(1), nav_state_j)\n",
    "values.insert(B(0), bias_i)\n",
    "\n",
    "error_vector = imu_factor2.evaluateError(nav_state_i, nav_state_j, bias_i)\n",
    "print(\"\\nError vector (should be near zero):\", error_vector)\n",
    "print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [ImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.h)\n",
    "- [ImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
